//
// Created by 86156 on 25-8-27.
//

#include "commander.h"

#include "math.h"

#include "rc_driver.h"
#include "control_driver.h"
#include "view_driver.h"

uint8_t BallHitted = 0;

void Normal_VSControl() {
    if (forward_back_flag == 1) TEST_mode.Forward_Speed = 1;
    else if (forward_back_flag == 0) TEST_mode.Forward_Speed = 0;
    else TEST_mode.Forward_Speed = -1;

    if (left_right_flag == 1) TEST_mode.Side_Speed = 1;
    else if (left_right_flag == 0) TEST_mode.Side_Speed = 0;
    else TEST_mode.Side_Speed = -1;


    if (up_down_flag == 1) {
        if (TEST_mode.Target_Depth >= 50)
        TEST_mode.Target_Depth -= 0.1;
    }
    else if (up_down_flag == 2) {
        if (TEST_mode.Target_Depth <= 600)
            TEST_mode.Target_Depth += 0.1;
    }
}

void Find_DoorFrame() {
    static uint8_t DoorFrameFlag = 0;
    static uint32_t DoorFrameTimer = 0;
    DoorFrameTimer++;
    if (DoorFrameTimer < 4000) {
        TEST_mode.Forward_Speed = -1;
    }
    else if (DoorFrameTimer>=4000 && DoorFrameTimer<6000) {
        TEST_mode.Forward_Speed = 0;
        TEST_mode.Target_Yaw = 0;
    }
    if (DoorFrameTimer >= 6000 && fabsf(Angle_Measure.yaw - TEST_mode.Target_Yaw) < 2) {
        BallHitted = 2;
    }
}

#define SECOND 1000
void commander_task() {
    static uint32_t commander_timer = 0;
    for (;;) {
        if (RC_MODE_FLAG == 1) {
            if (RC_Ctl.rc.s1 == RC_SW_UP) {
                TEST_mode.Side_Speed = RC_Ctl.back_one.channel0;
                TEST_mode.Forward_Speed = RC_Ctl.back_one.channel3;
                if (fabsf(RC_Ctl.back_one.channel2) > 0.1f)
                    TEST_mode.Target_Yaw += RC_Ctl.back_one.channel2 * 0.2f;
                Depth_Measure = RC_Ctl.back_one.channel1*100;
            }
        }
        else {


#ifdef Debug_Mode
            TEST_mode.Target_Yaw = -90;
            TEST_mode.Side_Speed = 1;
            commander_timer++;
            // if (commander_timer < 8*SECOND) {
            //     TEST_mode.Target_Yaw = -88;
            // }
            // else if (commander_timer >= 8*SECOND && commander_timer <= 16*SECOND) {
            //     TEST_mode.Forward_Speed = 1;
            // }
            // else if (commander_timer > 16*SECOND && commander_timer <= 21*SECOND) {
            //     TEST_mode.Forward_Speed = -1;
            // }
            // else if (commander_timer > 21*SECOND && commander_timer <= 25*SECOND) {
            //     TEST_mode.Forward_Speed = 0;
            //     TEST_mode.Target_Yaw = -5;
            // }
            // else if (commander_timer > 25*SECOND && commander_timer <= 45*SECOND) {
            //     TEST_mode.Forward_Speed = 1;
            // }
#else
            if (View_Connected == 0 || View_Connected == 2) {
                TEST_mode.Forward_Speed = 0;
                TEST_mode.Side_Speed = 0;
            }
            else {
                if (ProcessFlag == 0)
                {
                    TEST_mode.Target_Yaw = -90;
                    Normal_VSControl();
                }
                if (ProcessFlag == 1) { /// 准备
                    TEST_mode.Target_Yaw = -90;
                    Normal_VSControl();
                }
                else if (ProcessFlag == 2) {
                    if (BallHitted == 0) {
                        BallHitted = 1;
                    }
                    else if (BallHitted == 1) {
                        Find_DoorFrame();
                    }
                    else {
                        Normal_VSControl();
                    }
                }

            }
#endif

        }
        osDelay(1);
    }
}
